/****************************************************************************
 *  Copyright (C) 2018 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/
/** @file shoot_task.c
 *  @version 1.1
 *  @date June 2017
 *
 *  @brief shoot bullet task
 *
 *  @copyright 2017 DJI RoboMaster. All rights reserved.
 *
 */

#include "shoot_task.h"
#include "gimbal_task.h"
#include "detect_task.h"
#include "comm_task.h"
#include "modeswitch_task.h"
#include "remote_ctrl.h"
#include "bsp_io.h"
#include "bsp_can.h"
#include "bsp_uart.h"
#include "bsp_io.h"
#include "keyboard.h"
#include "pid.h"
#include "sys_config.h"
#include "cmsis_os.h"
#include "string.h"

/* stack usage monitor */
UBaseType_t shoot_stack_surplus;

/* shoot task global parameter */
shoot_t   shoot;
trigger_t trig;
extern user_judge_t user_judge_data;

#if (INFANTRY_NUM == INFANTRY_3)
	/*擦轮赋值*///最大2500
	int normal_speed = 2400;			//shoot:测试参数
	int mad_speed_a[4]		 = {1300, 1300, 1500, 1700};
	/*测试 弹丸射频*/
	int normal_shoot = -3000;			
	int normal_cshoot_a[4] = {-2000, -2000, -2500, -3000};
	int mad_shoot		 = -8050;
	int mad_cshoot_a[4]		 = {-3000, -3000, -5000, -7000};

#elif (INFANTRY_NUM == INFANTRY_4)
	/*擦轮赋值*/
	int normal_speed = 2400;			
	int mad_speed		 = 1300;
	/*测试 弹丸射频*/
	int normal_shoot = -3000;			
	int normal_cshoot= -3000;
	int mad_shoot		 = -8000;
	int mad_cshoot	 = -4000;
	
#elif (INFANTRY_NUM == INFANTRY_5)
	/*擦轮赋值*/
	int normal_speed = 2400;			
	int mad_speed		 = 1300;
	/*测试 弹丸射频*/
	int normal_shoot = -3000;			
	int normal_cshoot= -3000;
	int mad_shoot		 = -8000;
	int mad_cshoot	 = -4000;

#else
  #error "INFANTRY_NUM define error!"
#endif

uint32_t shoot_time_last;
int shoot_time_ms;
void shoot_task(void const *argu)
{
  osEvent event;
  
  while (1)
  {
		//等待信号量
    event = osSignalWait(SHOT_TASK_EXE_SIGNAL, osWaitForever);
    
    if (event.status == osEventSignal)
    {
      if (event.value.signals & SHOT_TASK_EXE_SIGNAL)
      {
        shoot_time_ms = HAL_GetTick() - shoot_time_last;
        shoot_time_last = HAL_GetTick();
				
        ball_storage_ctrl();					// 舵机控制弹仓盖（未设计舵机弹仓盖）
        mad_warrir_ctrl();						// 射击模式切换
				fric_wheel_ctrl();
        
        if (!shoot.fric_wheel_run)
        {
          shoot.shoot_cmd   = 0;
          shoot.c_shoot_cmd = 0;
        }
                
        //trig.key = get_trigger_key_state();
        
        if (shoot.fric_wheel_run)
        {
          shoot_bullet_handler();
        }
        else
        {
          pid_trigger_spd.out = 0;
        }
        
        trig.key_last = trig.key;
      }
    }
    
    shoot_stack_surplus = uxTaskGetStackHighWaterMark(NULL);
  }
}


 void block_bullet_handler(void)
{
	static uint32_t stall_mark = 0;
	static uint32_t stall_step = 0;
	
	if((stall_step == 0) && (pid_trigger_spd.iout <= -4900))
	{	
		stall_step = 1;
		stall_mark = HAL_GetTick();
	}
	if(stall_step == 1)
	{
		trig.spd_ref = 8000;
		if(HAL_GetTick() - stall_mark >= 70)
		{
		stall_step = 0;
		stall_mark = 0;
		}
	}
}

static void mad_warrir_ctrl(void)
{
	if (shoot.mad_mode_on && shoot.ctrl_mode != SEMIAUTO_CTRL_SHOT){		//shoot： 检查遥控控制模式
		shoot.fric_wheel_spd = mad_speed_a[user_judge_data.robot_level];
		trig.shoot_spd			 = mad_shoot;
		trig.c_shoot_spd		 = mad_cshoot_a[user_judge_data.robot_level];
	}
	else{
		shoot.fric_wheel_spd = normal_speed;
		trig.shoot_spd			 = normal_shoot;
		trig.c_shoot_spd		 = normal_cshoot_a[user_judge_data.robot_level];
		//trig.c_shoot_spd		 = normal_cshoot;
	}
}

static void fric_wheel_ctrl(void)
{
  if (shoot.fric_wheel_run)
  {
    turn_on_friction_wheel(shoot.fric_wheel_spd);
    turn_on_laser();
  }
  else
  {
    turn_off_friction_wheel();
    turn_off_laser();
  }
}

static void ball_storage_ctrl(void)
{
  if (shoot.ball_storage_open)
  {
    turn_on_ball_storage();
  }
  else
  {
    turn_off_ball_storage();
  }
}

int shoot_cmd;
int shoot_delay = 145;			//拨盘延时
static void shoot_bullet_handler(void)
{
  shoot_cmd = shoot.shoot_cmd;
  if (shoot.shoot_cmd)
  {
    if (trig.one_sta == TRIG_INIT)
		{
			trig.one_sta = TRIG_PRESS_DOWN;
			trig.one_time = HAL_GetTick();
    }
    else if (trig.one_sta == TRIG_PRESS_DOWN)
    {
      if (HAL_GetTick() - trig.one_time >= shoot_delay)//shoot_delay) 	//before the rising
      {
        trig.one_sta = TRIG_ONE_DONE;
      }
    }    
    if (trig.one_sta == TRIG_ONE_DONE)
    {
      trig.spd_ref = 0;
      trig.one_sta = TRIG_INIT;
      
      shoot.shoot_cmd = 0;
      shoot.shoot_bullets++;
    }
    else
      trig.spd_ref = trig.shoot_spd;
    
  }
  else if (shoot.c_shoot_cmd)
  {
    trig.one_sta = TRIG_INIT;
    trig.spd_ref = trig.c_shoot_spd;
    
    if ((trig.key_last == 0) && (trig.key == 1))
      shoot.shoot_bullets++;
    
    block_bullet_handler();
  }
	else
	{
    trig.spd_ref = 0;
	}
  
  pid_calc(&pid_trigger_spd, moto_trigger.speed_rpm, trig.spd_ref);
}

void shoot_param_init(void)
{
  memset(&shoot, 0, sizeof(shoot_t));
  
  shoot.ctrl_mode      = SHOT_DISABLE;
  shoot.fric_wheel_spd = DEFAULT_FRIC_WHEEL_SPEED;
  //shoot.remain_bullets = 0;
  
  memset(&trig, 0, sizeof(trigger_t));
  
  trig.shoot_spd			 = 0;
  trig.c_shoot_spd     = 0;
  trig.one_sta         = TRIG_INIT;
  
}

